/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.smartdashboard.SendablePIDController;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.Timer;
import java.util.TimerTask;

public class RobotTemplate extends IterativeRobot {
    private Joystick m_leftStick = new Joystick(1);
    private Joystick m_rightStick = new Joystick(2);
    private Joystick m_gigaware = new Joystick(3);

    private SpeedController m_frontLeftMotor = new Victor(2);
    private SpeedController m_rearLeftMotor = new Victor(1);
    private PIDOutput m_leftMotors = new PIDY(m_frontLeftMotor, m_rearLeftMotor);
    private Encoder m_leftEncoder = new RateEncoder(7, 8, 20, -20, kEncoderPeriod);
    private SendablePIDController m_leftPID = new SendablePIDController(kP, kI, kD, m_leftEncoder, m_leftMotors, kPIDPeriod);
    private PIDSpeedController m_leftPIDSpeedController = new PIDSpeedController(m_leftPID, kMaxSpeed, -kMaxSpeed);

    private SpeedController m_frontRightMotor = new Victor(3);
    private SpeedController m_rearRightMotor = new Victor(4);
    private PIDOutput m_rightMotors = new PIDNeg(new PIDY(m_frontRightMotor, m_rearRightMotor));
    private Encoder m_rightEncoder = new RateEncoder(5, 6, 20, -20, kEncoderPeriod);
    private SendablePIDController m_rightPID = new SendablePIDController(kP, kI, kD, m_rightEncoder, m_rightMotors, kPIDPeriod);
    private PIDSpeedController m_rightPIDSpeedController = new PIDSpeedController(m_rightPID, kMaxSpeed, -kMaxSpeed);

    private RobotDrive m_PIDdrivetrain = new Team997Drive(m_leftPIDSpeedController, m_rightPIDSpeedController, m_leftStick, m_rightStick);
    private RobotDrive m_normalDrivetrain = new Team997Drive(m_frontLeftMotor, m_rearLeftMotor, m_frontRightMotor, m_rearRightMotor, m_leftStick, m_rightStick);
    private JoyDrive m_PIDJoyDrive = new JoyDrive(m_PIDdrivetrain, m_leftStick, m_rightStick, m_gigaware, false);
    private JoyDrive m_normalJoyDrive = new JoyDrive(m_normalDrivetrain, m_leftStick, m_rightStick, m_gigaware, false);

    private Gyro m_gyro = new Gyro(2);
    private AnalogChannel m_sonar = new AnalogChannel(4);
//    private I2C m_accel = DigitalModule.getInstance(1).getI2C(0xA6);
    private ADXL345_I2C m_accel = new ADXL345_I2C(1, ADXL345_I2C.DataFormat_Range.k4G);
//    private SRF08Ultra m_ultrasonic = new SRF08Ultra(1, SRF08Ultra.kRangeModeInches);

    private boolean m_PIDEnabled = false;

    private static final double kP = 0.5;
    private static final double kI = 0.01;
    private static final double kD = 0.0;
    private static final double kMaxSpeed = 2.5;
    private static final double kDistancePerPulse = 1.0 / (360.0 * 2.17);
    private static final double kPIDPeriod = 0.050;
    private static final double kEncoderPeriod = kPIDPeriod - 0.005;
    private static final double kInchesPerVolt = 1.0 / 0.009766;

    public void robotInit() {
        m_leftEncoder.setPIDSourceParameter(Encoder.PIDSourceParameter.kRate);
        m_leftEncoder.setDistancePerPulse(kDistancePerPulse);

        m_rightEncoder.setPIDSourceParameter(Encoder.PIDSourceParameter.kRate);
        m_rightEncoder.setDistancePerPulse(kDistancePerPulse);

        m_leftEncoder.reset();
        m_leftEncoder.start();
        m_rightEncoder.reset();
        m_rightEncoder.start();

        m_leftPID.disable();
        m_rightPID.disable();

        Timer dashTimer = new Timer();

        m_gyro.reset();

//        if (m_accel.write(0x2D, 0x08))
//            System.out.println("Error writing to address 0x2D.");

        SmartDashboard.putData("Left PID", m_leftPID);
        SmartDashboard.putData("Right PID", m_rightPID);
        dashTimer.schedule(new TimerTask() {
            public void run() {
                
                updateSmartDashboard();
            }
        }, 0, 200);
    }

    public void disabledInit() {
        m_leftPID.disable();
        m_rightPID.disable();
    }

    public void teleopInit() {
        m_leftEncoder.reset();
        m_leftPID.reset();
        m_rightEncoder.reset();
        m_rightPID.reset();

        if (m_PIDEnabled) {
            m_leftPID.enable();
            m_rightPID.enable();
        } else {
            m_leftPID.disable();
            m_rightPID.disable();
        }

        m_normalDrivetrain.setSafetyEnabled(false);
        m_PIDdrivetrain.setSafetyEnabled(false);
    }

    public void teleopContinuous() {
        if (m_leftStick.getRawButton(11) || m_rightStick.getRawButton(11)) {
            m_leftPID.enable();
            m_rightPID.enable();
            m_PIDEnabled = true;
        } else if (m_leftStick.getRawButton(10) || m_rightStick.getRawButton(10)) {
            m_leftPID.disable();
            m_rightPID.disable();
            m_PIDEnabled = false;
        }

        getCurrentJoyDrive().update();
    }

    public JoyDrive getCurrentJoyDrive() {
        if (m_PIDEnabled)
            return m_PIDJoyDrive;
        else
            return m_normalJoyDrive;
    }

    private void updateSmartDashboard() {
        if (getCurrentJoyDrive().getDrivetrain() instanceof Team997Drive) {
            Team997Drive drivetrain = (Team997Drive) getCurrentJoyDrive().getDrivetrain();
            SmartDashboard.putDouble("Curve", drivetrain.getCurve());
            SmartDashboard.putDouble("Reverse Deadband", drivetrain.getReverseDeadband());
            SmartDashboard.putDouble("Deadband", drivetrain.getDeadband());
            SmartDashboard.putDouble("Motor Max", drivetrain.getMotorMax());
            SmartDashboard.putDouble("Left Motor Value", drivetrain.getLeftMotorValue());
            SmartDashboard.putDouble("Right Motor Value", drivetrain.getRightMotorValue());
        }

        SmartDashboard.putDouble("Left Distance", m_leftEncoder.getDistance());
        SmartDashboard.putDouble("Left Speed", m_leftEncoder.getRate());
        SmartDashboard.putDouble("Left Stick Y", m_leftStick.getY());

        SmartDashboard.putDouble("Right Distance", m_rightEncoder.getDistance());
        SmartDashboard.putDouble("Right Speed", m_rightEncoder.getRate());
        SmartDashboard.putDouble("Right Stick Y", m_rightStick.getY());

        SmartDashboard.putBoolean("PID Enabled", m_PIDEnabled);

        SmartDashboard.putDouble("Gyro", m_gyro.getAngle());
        SmartDashboard.putDouble("Sonar", m_sonar.getVoltage() * kInchesPerVolt);

//        m_ultrasonic.ping();
//        SmartDashboard.putDouble("Light Sensor", m_ultrasonic.getLight());
//        SmartDashboard.putDouble("Ultrasonic Sensor", m_ultrasonic.getRange());

        ADXL345_I2C.AllAxes accelValue = m_accel.getAccelerations();
        SmartDashboard.putDouble("Accel Axis X", accelValue.XAxis);
        SmartDashboard.putDouble("Accel Axis Y", accelValue.YAxis);
        SmartDashboard.putDouble("Accel Axis Z", accelValue.ZAxis);

//        byte[] buffer = new byte[1];
//        if (!m_accel.read(0x00, 1, buffer)) {
//            SmartDashboard.putInt("Byte 0", (int) (char) buffer[0]);
//        } else {
//            System.out.println("Error reading byte 0!");
//        }
    }
}